/*
 *	Filename: Quadrotor_main.c
 *	Fuction: Main Control
 *	Modified: 2016-11-09
 *	Author: Bin
 */

#include "DSP28x_Project.h"
#include "Quadrotor_eCap.h"
#include "Quadrotor_ePwm.h"
#include "Quadrotor_Mpu6050.h"

// Global Variable Definition
// Uint16 cap1 = 0, cap2 = 0, cap3 = 0, cap4 = 0;
Uint16 who = 0;
struct Mpu6050Data senData;

// Prototype statements for functions found within this file
// void updateCapValue(void) {
// 	cap1 = getCap1();
// 	cap2 = getCap2();
// 	cap3 = getCap3();
// 	cap4 = getCap4();
// }

void main(void) {
	
	// Step 1. Initialize System Control
	InitSysCtrl();

	// Step 2. Initalize GPIO
	InitECapGpioAll();
	InitEPwmGpioAll();
	InitI2cGpio();
	
	// Step 3. Clear all interrupts and initialize PIE vector table
	DINT;

	InitPieCtrl();

	IER = 0x0000;
	IFR = 0x0000;

	InitPieVectTable();

	// Set Interrupts that are used in Project
	EALLOW;
	PieVectTable.ECAP1_INT = &ecap1_isr;
	PieVectTable.ECAP2_INT = &ecap2_isr;
	PieVectTable.ECAP3_INT = &ecap3_isr;
	PieVectTable.ECAP4_INT = &ecap4_isr;
	EDIS;

	PieCtrlRegs.PIEIER4.bit.INTx1 = 1;
	PieCtrlRegs.PIEIER4.bit.INTx2 = 1;
	PieCtrlRegs.PIEIER4.bit.INTx3 = 1;
	PieCtrlRegs.PIEIER4.bit.INTx4 = 1;

	IER |= M_INT4;

	EINT;
	ERTM;

	// Step 4. Initialize all the Device Peripherals used in Project
	InitECapAll();
	InitEPwmAll();
	InitI2c();

	// Step 5. User specific code
	who = InitMpu6050();	
	DELAY_US(10000);	// Wait for Motor Initialization

	while(1) {
		getSensor(&senData);
		DELAY_US(1000);
		// updateCapValue();

		// if(cap4 < 1500) {
		// 	setEPwm1A(0);
		// 	setEPwm1B(0);
		// 	setEPwm2A(0);
		// 	setEPwm2B(0);
		// 	while(cap4 < 1500) updateCapValue();
		// }
		
		// setEPwm1A(cap2);
		// setEPwm1B(cap1);
	}
}
// End of Main Fuction
